
#include <stdio.h>
#include "TCLStereoDepthAlg.h"

#define CV_CPU_HAS_SUPPORT_NEON 1
#include "opencv2/core/hal/intrin.hpp"

tcl_depth_video::TCLStereoDepthAlg_prv::TCLStereoDepthAlg_prv() {}

tcl_depth_video::TCLStereoDepthAlg_prv::~TCLStereoDepthAlg_prv() {
    this->task_done = true;
    this->thread_state = THREAD_STATE_QUIT;
    VIDEO_DEPTH_LOGI("EXIT SIGNAL SENT!\n");
    //pthread_join(thread_core, NULL);
	thread_core.join();
    VIDEO_DEPTH_LOGI("FINISHED!\n");
}


void *tcl_depth_video::DepthPreview_Thread(void * parent){
    cv::setNumThreads(0);

    double freq = cv::getTickFrequency();
    double start, stop;
    TCLStereoDepthAlg_prv *_this = (TCLStereoDepthAlg_prv *)(parent);
    while ( (!(_this->task_done)) && (_this->thread_state!=THREAD_STATE_QUIT) ){
        if( !_this->session_done ){
            if ( _this->thread_state!=THREAD_STATE_PLAY ) {
                _this->session_done = true;
                continue;
            }
            start = TICK();
            if (!_this->CalcDenseOptFlow()) {
                _this->session_done = true;
                continue;
            }
            stop = TICK();
            VIDEO_DEPTH_LOGI("THREAD->CalcDenseOptFlow: %6.3fms", 1000*(stop-start)/freq);

            _this->session_done = true;
            VIDEO_DEPTH_LOGW("THREAD->NUMBER: %d", cv::getNumThreads());
        }
    }
	return nullptr;
}


void tcl_depth_video::TCLStereoDepthAlg_prv::Init(const Param_DepthPreview & param) {
    VIDEO_DEPTH_LOGI("INIT START");
    double start, start_o, stop, freq = cv::getTickFrequency();
    start_o = start = TICK();

#ifdef _WIN32
	std::cout << "M1 = " << param.intrinsics.M1 << std::endl;
	std::cout << "D1 = " << param.intrinsics.D1 << std::endl;
	std::cout << "M2 = " << param.intrinsics.M2 << std::endl;
	std::cout << "D2 = " << param.intrinsics.D2 << std::endl;
	//PAUSE();
#endif

    this->m_param.copy(param);
    depth_out.create(m_param.depth_size, CV_8UC1);
    depth_last.create(m_param.depth_size, CV_8UC1);
	depth_base.create(m_param.depth_size, CV_8UC1);
    stop = TICK();
    VIDEO_DEPTH_LOGI("INIT IMAGES: %6.3fms", 1000*(stop-start)/freq);
    start = stop;

    success_flag = true;
    task_done = false;
    session_done = true;
    map_locked = false;
    freeze_times = 0;
    thread_state = THREAD_STATE_PLAY;
    rectify_size.width = 480;
    rectify_size.height = 360;
    wls_size.width = 160;
    wls_size.height = 120;
    m_param.max_disp = 16; // 16
    m_param.win_size = 15; // 15
    m_param.skip_cycle = 6;

	size_close = cv::Size(rectify_size.width / 2, rectify_size.height / 2);
	size_micro = cv::Size(rectify_size.width / 4, rectify_size.height / 4);
	disp_close = 1.0 * m_param.max_disp;
	disp_micro = 2.0 * m_param.max_disp;

	if (m_param.orientation == TCL_IMAGE_ROTATE_COUNTER_CLOCKWISE_90 ||
		m_param.orientation == TCL_IMAGE_ROTATE_CLOCKWISE_90) {
		m_param.full_size = cv::Size(m_param.full_size.height, m_param.full_size.width);
		m_param.crop_size = cv::Size(m_param.crop_size.height, m_param.crop_size.width);
		rectify_size = cv::Size(rectify_size.height, rectify_size.width);
	}

    cali_w = std::fmax(m_param.cali_size.width, 1);
    cali_h = std::fmax(m_param.cali_size.height, 1);
    full_w = std::fmax(m_param.full_size.width, 1);
    full_h = std::fmax(m_param.full_size.height, 1);
    crop_w = std::fmax(m_param.crop_size.width, 1);
    crop_h = std::fmax(m_param.crop_size.height, 1);
    dept_w = rectify_size.width;
    dept_h = rectify_size.height;

    crop_cali_size.width = (int)(cali_w-(cali_w/full_w)*(full_w-crop_w));
    crop_cali_size.height = (int)(cali_h-(cali_h/full_h)*(full_h-crop_h));
    
	down_scale = dept_w / (float)crop_cali_size.width;

	double fx1, fx2, fy1, fy2;
	fx1 = m_param.intrinsics.M1.at<double>(0, 0);
	fy1 = m_param.intrinsics.M1.at<double>(1, 1);
	fx2 = m_param.intrinsics.M2.at<double>(0, 0);
	fy2 = m_param.intrinsics.M2.at<double>(1, 1);
	scale_aux_x = down_scale * fx2 / fx1;
	scale_aux_y = down_scale * fy2 / fy1;

    crop_delta_size.width = down_scale * cali_w - dept_w;
    crop_delta_size.height = down_scale * cali_h - dept_h;

	// step 1. scale down main to depth size, 
	// and scale down aux to the same zoom level of main in depth size.
	// update intrinsics for aux camera
	m_param.intrinsics.M1.at<double>(0, 0) *= scale_aux_x;
	m_param.intrinsics.M1.at<double>(0, 2) *= scale_aux_x;
	m_param.intrinsics.M1.at<double>(1, 1) *= scale_aux_y;
	m_param.intrinsics.M1.at<double>(1, 2) *= scale_aux_y;

	// update intrinsics for main camera
	m_param.intrinsics.M2.at<double>(0, 0) *= down_scale;
	m_param.intrinsics.M2.at<double>(0, 2) = m_param.intrinsics.M2.at<double>(0, 2) * down_scale - crop_delta_size.width / 2.0;
	m_param.intrinsics.M2.at<double>(1, 1) *= down_scale;
	m_param.intrinsics.M2.at<double>(1, 2) = m_param.intrinsics.M2.at<double>(1, 2) * down_scale - crop_delta_size.height / 2.0;

    stop = TICK();
    VIDEO_DEPTH_LOGI("INIT VARS: %6.3fms", 1000*(stop-start)/freq);
    start = stop;

    cv::Mat E = cv::Mat::diag(cv::Mat::ones(cv::Size(1, 3), CV_64F));
    cv::Mat D = cv::Mat::zeros(cv::Size(8, 1), CV_64F);

	cv::Size aux_size;
	aux_size.width = scale_aux_x * cali_w;
	aux_size.height = scale_aux_y * cali_h;

    // normalize both views
	try {
		cv::initUndistortRectifyMap(m_param.intrinsics.M1,
									m_param.intrinsics.D1,
									E,
									m_param.intrinsics.M1,
									aux_size,
									CV_32F,
									map11, map12);
		cv::initUndistortRectifyMap(m_param.intrinsics.M2,
									m_param.intrinsics.D2,
									E,
									m_param.intrinsics.M2,
									rectify_size,
									CV_32F,
									map21, map22);
	}
	catch (...) {
		init_success = false;
		VIDEO_DEPTH_LOGE("Init Failed: illegal calibration data!");
		return;
	}

    stop = TICK();
    VIDEO_DEPTH_LOGI("INIT RECTIFY MAP: %6.3fms", 1000*(stop-start)/freq);
    start = stop;

    // set Q which we may need
    /*cv::stereoRectify(m_param.intrinsics.M2, D,
                      m_param.intrinsics.M2, D,
                      rectify_size,
                      m_param.extrinsics.R,
                      m_param.extrinsics.T,
                      R1, R2, P1, P2, Q,
                      CALIB_ZERO_DISPARITY, 1,
                      rectify_size);

    stop = TICK();
    VIDEO_DEPTH_LOGI("INIT MATRIX Q: %6.3fms", 1000*(stop-start)/freq);
    start = stop;
	*/

    bm = cv::StereoBM::create(m_param.max_disp, m_param.win_size);
	bm->setUniquenessRatio(30);
	bm->setSpeckleRange(800);
	bm->setSpeckleWindowSize(32);

    wls_filter = cv::ximgproc::createDisparityWLSFilterGeneric(false);
    wls_filter->setSigmaColor(3);
    wls_filter->setLambda(800);

    stop = TICK();
    VIDEO_DEPTH_LOGI("INIT WLS FILTER: %6.3fms", 1000*(stop-start)/freq);
    start = stop;

    // start a thread to support depth computation core task
	thread_core = std::thread(DepthPreview_Thread, this);

    stop = TICK();
    VIDEO_DEPTH_LOGI("START DEPTH THREAD: %6.3fms", 1000*(stop-start)/freq);
    start = stop;

	init_success = true;
    VIDEO_DEPTH_LOGI("INIT DONE: %6.3fms", 1000*(stop-start_o)/freq);
}


void tcl_depth_video::TCLStereoDepthAlg_prv::SetupInput(const LPTCL_IMAGE aux, const LPTCL_IMAGE main){
    double start, stop, freq = cv::getTickFrequency();
    start = TICK();
	sample_NV12_2x2(inputGray1, inputU1, inputV1, aux);
	sample_NV12_2x2(inputGray2, inputU2, inputV2, main);
    stop = TICK();
    VIDEO_DEPTH_LOGI("SetupInput: %6.3fms", 1000*(stop-start)/freq);
}


int tcl_depth_video::TCLStereoDepthAlg_prv::Preprocess() {
    VIDEO_DEPTH_LOGI("Preprocess BEGIN");
    double start, start_o, stop, freq = cv::getTickFrequency();
    start_o = TICK();
    // auxiliary camera
    start = TICK();
    if (thread_state!=THREAD_STATE_PLAY) return 0;

	if (m_param.dump_enabled) DumpImage(inputGray1, m_param.dump_path, "aux_0", uid);
	if (m_param.dump_enabled) DumpImage(inputGray2, m_param.dump_path, "main_0", uid);

	// for aux camera
	cv::Size size_small = cv::Size((int)(scale_aux_x * cali_w), (int)(scale_aux_y * cali_h));
	if (m_param.orientation == TCL_IMAGE_ROTATE_NONE) {
		cv::resize(inputGray1, resizeImg1, size_small);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_180) {
		cv::resize(inputGray1, resizeImg1, size_small);
		cv::rotate(resizeImg1, resizeImg1, cv::ROTATE_180);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_COUNTER_CLOCKWISE_90) {
		cv::Size tmp_size(size_small.height, size_small.width);
		cv::resize(inputGray1, resizeImg1_T, tmp_size);
		cv::rotate(resizeImg1_T, resizeImg1, cv::ROTATE_90_COUNTERCLOCKWISE);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_CLOCKWISE_90) {
		cv::Size tmp_size(size_small.height, size_small.width);
		cv::resize(inputGray1, resizeImg1_T, tmp_size);
		cv::rotate(resizeImg1_T, resizeImg1, cv::ROTATE_90_CLOCKWISE);
	}
	stop = TICK();
	VIDEO_DEPTH_LOGI("aux resize and rotate: %6.3fms", 1000 * (stop - start) / freq);

	/*
	if (m_param.orientation == TCL_IMAGE_ROTATE_NONE) {
		ScaleDownAndCenterCrop(
			resizeImg1,
			inputGray1, 
			down_scale, 
			m_param.cali_size,
			crop_delta_size);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_180) {
		ScaleDownAndCenterCrop(
			resizeImg1,
			inputGray1,
			down_scale,
			m_param.cali_size,
			crop_delta_size);
		cv::rotate(resizeImg1, resizeImg1, cv::ROTATE_180);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_COUNTER_CLOCKWISE_90) {
		cv::Size tmp_size_cali, tmp_size_delta;
		tmp_size_cali.width = m_param.cali_size.height;
		tmp_size_cali.height = m_param.cali_size.width;
		tmp_size_delta.width = crop_delta_size.height;
		tmp_size_delta.height = crop_delta_size.width;
		ScaleDownAndCenterCrop(
			resizeImg1_T, 
			inputGray1, 
			down_scale,
			tmp_size_cali, 
			tmp_size_delta);
		cv::rotate(resizeImg1_T, resizeImg1, cv::ROTATE_90_COUNTERCLOCKWISE);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_CLOCKWISE_90) {
		cv::Size tmp_size_cali, tmp_size_delta;
		tmp_size_cali.width = m_param.cali_size.height;
		tmp_size_cali.height = m_param.cali_size.width;
		tmp_size_delta.width = crop_delta_size.height;
		tmp_size_delta.height = crop_delta_size.width;
		ScaleDownAndCenterCrop(
			resizeImg1_T,
			inputGray1,
			down_scale,
			tmp_size_cali,
			tmp_size_delta);
		cv::rotate(resizeImg1_T, resizeImg1, cv::ROTATE_90_CLOCKWISE);
	}
	*/
    
    // main camera
    if (thread_state!=THREAD_STATE_PLAY) return 0;
	if (m_param.orientation == TCL_IMAGE_ROTATE_NONE) {
		cv::resize(inputGray2, resizeImg2, rectify_size);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_180) {
		cv::resize(inputGray2, resizeImg2, rectify_size);
		cv::rotate(resizeImg2, resizeImg2, cv::ROTATE_180);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_COUNTER_CLOCKWISE_90) {
		cv::Size tmp_size_rectify;
		tmp_size_rectify.width = rectify_size.height;
		tmp_size_rectify.height = rectify_size.width;
		cv::resize(inputGray2, resizeImg2_T, tmp_size_rectify);
		cv::rotate(resizeImg2_T, resizeImg2, cv::ROTATE_90_COUNTERCLOCKWISE);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_CLOCKWISE_90) {
		cv::Size tmp_size_rectify;
		tmp_size_rectify.width = rectify_size.height;
		tmp_size_rectify.height = rectify_size.width;
		cv::resize(inputGray2, resizeImg2_T, tmp_size_rectify);
		cv::rotate(resizeImg2_T, resizeImg2, cv::ROTATE_90_CLOCKWISE);
	}
    
	stop = TICK();
    VIDEO_DEPTH_LOGI("main resize and rotate: %6.3fms", 1000*(stop-start)/freq);

    VIDEO_DEPTH_LOGI("Preprocess DONE: %6.3fms", 1000*(stop-start_o)/freq);

	if (m_param.dump_enabled) DumpImage(resizeImg1, m_param.dump_path, "aux_1", uid);
	if (m_param.dump_enabled) DumpImage(resizeImg2, m_param.dump_path, "main_1", uid);

    return 1;
}


int tcl_depth_video::TCLStereoDepthAlg_prv::ComputeDepthImpl(
        const cv::Mat & main_,
        const cv::Mat & aux_
) {
    VIDEO_DEPTH_LOGI("ComputeDepthImpl START");
    double freq = cv::getTickFrequency();
    double start, stop;
	// special pipeline for micro-distance mode
	main_grey = main_;
	aux_grey = aux_;
	if (real_max_disp >= disp_micro) {
		VIDEO_DEPTH_LOGI("=================== MICRO mode =====================");
		cv::resize(main_grey, main_micro, size_micro);
		cv::resize(aux_grey, aux_micro, size_micro);
		// generate raw depth map
		start = TICK();
		if (thread_state != THREAD_STATE_PLAY) return 0;
		bm->compute(main_micro, aux_micro, depth_s16_micro);
		//depth_s16_micro = depth_s16_micro * 4;
		//cv::threshold(depth_s16_micro, depth_s16_micro, real_max_disp * 16, 0, cv::THRESH_TOZERO_INV);
		if (thread_state != THREAD_STATE_PLAY) return 0;
		//cv::threshold(depth_s16_micro, depth_s16_micro, (m_param.max_disp - 1) * 16, (m_param.max_disp - 1) * 16, cv::THRESH_TRUNC);
		depth_s16_micro.convertTo(depth_u8_micro, CV_8UC1, 255.0 / (16 * m_param.max_disp), 0);
		FillBlackHoles_Gray(depth_u8_micro, main_micro);
		cv::resize(depth_u8_micro, depth_u8, rectify_size);
	}
	else if (real_max_disp >= disp_close) {
		VIDEO_DEPTH_LOGI("=================== CLOSE mode =====================");
		cv::resize(main_grey, main_close, size_close);
		cv::resize(aux_grey, aux_close, size_close);
		// generate raw depth map
		start = TICK();
		if (thread_state != THREAD_STATE_PLAY) return 0;
		bm->compute(main_close, aux_close, depth_s16_close);
		//depth_s16_close = depth_s16_close * 2;
		cv::threshold(depth_s16_close, depth_s16_close, (real_max_disp/2 + 1) * 16, 0, cv::THRESH_TOZERO_INV);
		if (thread_state != THREAD_STATE_PLAY) return 0;
		//cv::threshold(depth_s16_close, depth_s16_close, (m_param.max_disp - 1) * 16, (m_param.max_disp - 1) * 16, cv::THRESH_TRUNC);
		depth_s16_close.convertTo(depth_u8_close, CV_8UC1, 255.0 / (16 * m_param.max_disp), 0);
		FillBlackHoles_Gray(depth_u8_close, main_close);
		cv::resize(depth_u8_close, depth_u8, rectify_size);
	}
	else {
		// generate raw depth map
		start = TICK();
		if (thread_state != THREAD_STATE_PLAY) return 0;
		bm->compute(main_grey, aux_grey, depth_s16);
		cv::threshold(depth_s16, depth_s16, real_max_disp * 16, 0, cv::THRESH_TOZERO_INV);
		if (thread_state != THREAD_STATE_PLAY) return 0;
		depth_s16.convertTo(depth_u8, CV_8UC1, 255.0 / (16 * m_param.max_disp), 0);
	}
    stop = TICK();
    VIDEO_DEPTH_LOGI("Match: %.3f ms", 1000*(stop-start) / freq); // 70ms
    if ( m_param.dump_enabled ) DumpImage(depth_u8, m_param.dump_path, "disp_0", uid);

    VIDEO_DEPTH_LOGI("ComputeDepthImpl() end"); // total= 111ms
    return 1;
}


int tcl_depth_video::TCLStereoDepthAlg_prv::ComputeDepth(){
    VIDEO_DEPTH_LOGI("ComputeDepth START");
    double start, stop, freq = cv::getTickFrequency();
    start = TICK();
    if (thread_state!=THREAD_STATE_PLAY) return 0;
    cv::flip(stdImg1, stdImg1, -1);
    if (thread_state!=THREAD_STATE_PLAY) return 0;
    cv::flip(stdImg2, stdImg2, -1);

    if (thread_state!=THREAD_STATE_PLAY) return 0;
    if (!ComputeDepthImpl(stdImg2, stdImg1)) return 0; 

    if (thread_state!=THREAD_STATE_PLAY) return 0;
    cv::flip(depth_u8, depth_u8, -1);
    //cv::remap(depth_u8, depth_u8, inv_map1, inv_map2, INTER_LINEAR);
    stop = TICK();
    VIDEO_DEPTH_LOGI("ComputeDepth DONE: %6.3f ms", 1000*(stop-start) / freq); 
    return 1;
};


int tcl_depth_video::TCLStereoDepthAlg_prv::Postprocess() {
    double freq = cv::getTickFrequency();
    double start, stop;
    cv::Mat disp_1, disp_2, main_wls;
    // resize into smaller disparity map and apply smooth filter
    start = TICK();
    if (thread_state!=THREAD_STATE_PLAY) return 0;
	if (m_param.orientation == TCL_IMAGE_ROTATE_NONE) {
		cv::resize(depth_u8, disp_1, wls_size);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_180) {
		cv::resize(depth_u8, disp_1, wls_size);
		cv::rotate(disp_1, disp_1, cv::ROTATE_180);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_COUNTER_CLOCKWISE_90) {
		cv::Size tmp_size_wls;
		tmp_size_wls.width = wls_size.height;
		tmp_size_wls.height = wls_size.width;
		cv::resize(depth_u8, disp_1, tmp_size_wls);
		cv::rotate(disp_1, disp_1, cv::ROTATE_90_CLOCKWISE);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_CLOCKWISE_90) {
		cv::Size tmp_size_wls;
		tmp_size_wls.width = wls_size.height;
		tmp_size_wls.height = wls_size.width;
		cv::resize(depth_u8, disp_1, tmp_size_wls);
		cv::rotate(disp_1, disp_1, cv::ROTATE_90_COUNTERCLOCKWISE);
	}
    if (thread_state!=THREAD_STATE_PLAY) return 0;
	// process the guide RGB image
	if (m_param.orientation == TCL_IMAGE_ROTATE_NONE) {
		cv::resize(main_grey, main_wls, wls_size);
		cv::flip(main_wls, main_wls, -1);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_180) {
		cv::resize(main_grey, main_wls, wls_size);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_COUNTER_CLOCKWISE_90) {
		cv::Size tmp_size_wls;
		tmp_size_wls.width = wls_size.height;
		tmp_size_wls.height = wls_size.width;
		cv::resize(main_grey, main_wls, tmp_size_wls);
		cv::rotate(main_wls, main_wls, cv::ROTATE_90_COUNTERCLOCKWISE);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_CLOCKWISE_90) {
		cv::Size tmp_size_wls;
		tmp_size_wls.width = wls_size.height;
		tmp_size_wls.height = wls_size.width;
		cv::resize(main_grey, main_wls, tmp_size_wls);
		cv::rotate(main_wls, main_wls, cv::ROTATE_90_CLOCKWISE);
	}

    if (thread_state!=THREAD_STATE_PLAY) return 0;
    stop = TICK();
    VIDEO_DEPTH_LOGI("resize: %6.3f ms", 1000*(stop-start) / freq);
    if ( m_param.dump_enabled )
        DumpImage(disp_1, m_param.dump_path, "disp_1", uid);

    // remove white spots
    start = TICK();
    if (thread_state!=THREAD_STATE_PLAY) return 0;
    HistBasedPeakFilter(disp_1, 0.007, 16);
    stop = TICK();
    VIDEO_DEPTH_LOGI("HistBasedPeakFilter: %.3f ms", 1000*(stop-start) / freq); // 3ms
    if ( m_param.dump_enabled )
        DumpImage(disp_1, m_param.dump_path, "disp_2", uid);

    // fill the rest black holes
    start = TICK();
    if (thread_state!=THREAD_STATE_PLAY) return 0;
    FillBlackHoles_Gray(disp_1, main_wls);
    FillBlackHoles_Gray(disp_1, main_wls);
    stop = TICK();
    VIDEO_DEPTH_LOGI("FillBlackHoles: %.3f ms", 1000*(stop-start) / freq); // 3ms
    if ( m_param.dump_enabled )
        DumpImage(disp_1, m_param.dump_path, "disp_3", uid);

    // use wls filter to smooth disparity
    start = TICK();
    wls_filter->setSigmaColor(3);
    wls_filter->setLambda(800);
    if (thread_state!=THREAD_STATE_PLAY) return 0;
    wls_filter->filter(disp_1, main_wls, disp_2);
    if ( m_param.dump_enabled )
        DumpImage(disp_2, m_param.dump_path, "disp_4", uid);

    // smooth the disparity using self-conducted wls
    wls_filter->setSigmaColor(0.4);
    wls_filter->setLambda(8000);
    if (thread_state!=THREAD_STATE_PLAY) return 0;
    wls_filter->filter(disp_2, disp_2, disp_1);

    stop = TICK();
    VIDEO_DEPTH_LOGI("WLS: %6.3f ms", 1000*(stop-start) / freq);
    if ( m_param.dump_enabled )
        DumpImage(disp_1, m_param.dump_path, "disp_5", uid);

	HistBasedFilter(disp_1, 0.04, false);

    start = TICK();
    cv::resize(disp_1, depth_base, m_param.depth_size);
    stop = TICK();
    VIDEO_DEPTH_LOGI("resize: %6.3f ms", 1000*(stop-start) / freq);

	//if (thread_state!=THREAD_STATE_PLAY) return 0;
	//while (map_locked);
	//map_locked = true;
	//depth_base.copyTo(depth_last);
	//map_locked = false;

    VIDEO_DEPTH_LOGI("PREVIEW->DISPARITY->POSTPROCESS DONE!");
    return 1;
}


float tcl_depth_video::TCLStereoDepthAlg_prv::Compute(
        cv::Mat & depth,
        LPTCL_IMAGE yuv_aux,
        LPTCL_IMAGE yuv_main,
        const cv::String & uid,
        float focus_x,
        float focus_y,
        int flag
){
    this->uid = uid;
	float dist = 1500; //GetFocusDistance(focus_x, focus_y);

    if (!flag) {
        VIDEO_DEPTH_LOGW("frame skipped");
        thread_state = THREAD_STATE_WAIT;
        //depth_last.copyTo(depth);
        return dist;
    } else thread_state = THREAD_STATE_PLAY;

    if (this->session_done) {
        // using focus distance to fix lens breathing
        SetupInput(yuv_aux, yuv_main);
        session_done = false;
    }
    // synchronize on the depth map buffer
	if (!success_flag) { 
		VIDEO_DEPTH_LOGW("no depth effect");
		return -1.0F;
	}
    while (map_locked);
    map_locked = true;
    depth_last.copyTo(depth);
    //if(m_param.dump_enabled)
    //    DumpImage(depth_last, m_param.dump_path, "tcl_depth_prev", this->uid);
    map_locked = false;

    return dist;
}


int tcl_depth_video::TCLStereoDepthAlg_prv::StereoRectify(){
    VIDEO_DEPTH_LOGI("StereoRectify START");
    
    double start, start_o, stop, freq = cv::getTickFrequency();
    start_o = start = TICK();

	// main camera
	if (thread_state != THREAD_STATE_PLAY) return 0;
	cv::remap(resizeImg2, cropImg2, map21, map22, cv::INTER_LINEAR);
	if (thread_state != THREAD_STATE_PLAY) return 0;
	cv::GaussianBlur(cropImg2, stdImg2, Size(3, 3), 2, 2);

	// sub camera
	if (thread_state != THREAD_STATE_PLAY) return 0;
	cv::remap(resizeImg1, remapImg1, map11, map12, cv::INTER_LINEAR);

	// crop to align size of sub to that of main
	cv::Size size_small = cv::Size((int)(scale_aux_x * cali_w), (int)(scale_aux_y * cali_h));
	//VIDEO_DEPTH_LOGI("size_small: %d, %d", size_small.width, size_small.height);
	//VIDEO_DEPTH_LOGI("size of resizeImg1: %d, %d", resizeImg1.size().width, resizeImg1.size().height);

	float crop_x, crop_y;
	crop_x = (size_small.width - rectify_size.width) / 2.0;
	crop_y = (size_small.height - rectify_size.height) / 2.0;
	
	if (crop_x < 0 || crop_y < 0) {
		VIDEO_DEPTH_LOGE("\t cropping exceeding range of aux camera!");
		VIDEO_DEPTH_LOGE("\t crop_x: %6.1f crop_y: %6.1f", crop_x, crop_y);
		if (cropImg1.size() != rectify_size) cropImg1 = cv::Mat::zeros(rectify_size, CV_8UC1);
		cv::Rect2f src_roi, dst_roi;
		src_roi.x = std::fmax(0, crop_x);
		src_roi.y = std::fmax(0, crop_y);
		src_roi.width = remapImg1.size().width - 2 * src_roi.x;
		src_roi.height = remapImg1.size().height - 2 * src_roi.y;
		dst_roi.x = std::fmax(0, -crop_x);
		dst_roi.y = std::fmax(0, -crop_y);
		dst_roi.width = src_roi.width;
		dst_roi.height = src_roi.height;
		remapImg1(src_roi).copyTo(cropImg1(dst_roi));
	}
	else {
		cropImg1 = remapImg1(cv::Rect(
			crop_x,
			crop_y,
			rectify_size.width,
			rectify_size.height)).clone();
	}
	
	if (thread_state != THREAD_STATE_PLAY) return 0;
	cv::GaussianBlur(cropImg1, blurImg1, Size(3, 3), 2, 2);

    stop = TICK();
    VIDEO_DEPTH_LOGI("StereoRectify->remap: %6.3fms", 1000*(stop-start)/freq);
    if (m_param.dump_enabled)
        DumpImage(blurImg1, m_param.dump_path, "aux_2", uid);
    if (m_param.dump_enabled)
        DumpImage(stdImg2, m_param.dump_path, "main_2", uid);

    int num_feat = 1000;
    int max_iter = 300;
    float res_level = 0.5;
    int num_worst = 3;
    if (thread_state!=THREAD_STATE_PLAY) return 0;
    int status_ = RowAlign_grid_tws(
            stdImg1, stdImg2, blurImg1,
            num_feat, max_iter, res_level, num_worst,
            aux_map_x, aux_map_y, true,
            &thread_state, success_flag,
            real_max_disp, disp_spa, coord_spa);
    if (!status_){
        VIDEO_DEPTH_LOGE("row align failed!");
        blurImg1.copyTo(stdImg1);
        return 0;
    }
    stop = TICK();
    VIDEO_DEPTH_LOGI("StereoRectify->RowAlign_grid_tws: %6.3fms", 1000*(stop-start)/freq);
    if (m_param.dump_enabled)
        DumpImage(stdImg1, m_param.dump_path, "aux_3", uid);
    if (m_param.dump_enabled)
        DumpImage(stdImg2, m_param.dump_path, "main_3", uid);
    stop = TICK();
    VIDEO_DEPTH_LOGI("StereoRectify DONE: %6.3fms", 1000*(stop-start_o)/freq);
    return 1;
}


//float TCLStereoDepthAlg_prv::GetFocusDistance(
//        float focus_x,
//        float focus_y
//) {
//    while (map_locked);
//    map_locked = true;
//    if (focus_x<0 || focus_x>=1 || focus_y<0 || focus_y>=1){
//        VIDEO_DEPTH_LOGE("[GetFocusDistance] ASSERTION FAILURE!");
//		FAIL_EXIT(1);
//    }
//    Mat point_(4, 1, CV_64F);
//    float x_, y_, d_;
//    int radius = 3; // the radius to search the depth of focus
//    float sum_of_depth;
//    int begin_x, begin_y, end_x, end_y;
//
//    x_ = (1 - focus_x) * depth_last.size().width;
//    y_ = (1 - focus_y) * depth_last.size().height;
//    begin_x = max(0, int(x_) - radius);
//    begin_y = max(0, int(y_) - radius);
//    end_x = min(depth_last.size().width, int(x_) + radius);
//    end_y = min(depth_last.size().height, int(y_) + radius);
//    sum_of_depth = 0.f;
//
//    for (int i = begin_y; i<end_y; ++i) {
//        for (int j = begin_x; j<end_x; ++j) {
//            sum_of_depth += depth_last.at<unsigned char>(i, j);
//        }
//    }
//    map_locked = false; // release lock
//
//    d_ = sum_of_depth / ((end_x - begin_x) * (end_x - begin_x));
//    d_ = d_ * 16.0 * m_param.max_disp / 255.0;
//
//    if ( d_ < 1) return 1E6;
//
//    point_.at<double>(0, 0) = x_;
//    point_.at<double>(1, 0) = y_;
//    point_.at<double>(2, 0) = d_;
//    point_.at<double>(3, 0) = 1.0F;
//    Mat physic_point = Q * point_;
//    float dis = physic_point.at<double>(0, 2) / \
//    physic_point.at<double>(0, 3);
//    return dis * 16;
//}


//int TCLStereoDepthAlg_prv::Depth2Disparity(float mm) {
//    float disp = (Q.at<double>(2, 3) - mm * Q.at<double>(3, 3)) /\
//     (mm * Q.at<double>(3, 2));
//    return (int)std::floor(disp * 255 / m_param.max_disp);
//}


//void TCLStereoDepthAlg_prv::Disparity2Depth(cv::Mat &depthOut, const cv::Mat &dispIn){
//    double d_;
//    if (depthOut.size()!=dispIn.size() || depthOut.type()!=CV_8UC1){
//        depthOut.create(dispIn.size(), CV_8UC1);
//    }
//
//    int h = dispIn.size().height;
//    int w = dispIn.size().width;
//
//    const uint8_t *dispInData = (uint8_t*)(dispIn.data);
//    uint8_t *depthOutData = (uint8_t*)(depthOut.data);
//
//    double f = Q.at<double>(2,3);
//    double Tx = 1/Q.at<double>(3,2);
//    double K = f*Tx;
//    double C1 = 255.0 / (3500-80);
//
//    for (int i=0; i<h; ++i){
//        for (int j=0; j<w; ++j){
//            int id = i * w + j;
//            d_ = dispInData[id] * m_param.max_disp / 255.0 + 1E-5;
//            d_ = (K / d_ - 80) * C1;
//            depthOutData[id] = (uint8_t)std::max(0.0, std::min(d_, 255.0));
//        }
//    }
//
//}


int tcl_depth_video::TCLStereoDepthAlg_prv::CalcDenseOptFlow(){
	if (!init_success) {
		VIDEO_DEPTH_LOGE("Not available: module uninitialized or initialization failed!");
		if (depth_base.size()!= m_param.depth_size) {
			depth_base = cv::Mat::zeros(m_param.depth_size, CV_8UC1) + 255;
			while (map_locked);
			map_locked = true;
			depth_base.copyTo(depth_last);
			map_locked = false;
		}
		return 1;
	}

    cv::Size opt_size(240, 180);
    double start, stop, freq = cv::getTickFrequency();
    if (prev.empty()) {
		VIDEO_DEPTH_LOGI("====== FIRST RUN ======");
        start = TICK();
        cv::resize(inputGray2, prev, opt_size);
        stop = TICK();
        VIDEO_DEPTH_LOGI("CalcDenseOptFlow->Preprocess: %6.3fms", 1000*(stop-start)/freq);
        // run dualcam depth estimate algorithm
        start = TICK();
        if (!Preprocess()) return 0;
        stop = TICK();
        VIDEO_DEPTH_LOGI("THREAD->Preprocess: %6.3fms", 1000*(stop-start)/freq);
        start = stop;
        if (!StereoRectify()) return 0;
        stop = TICK();
        VIDEO_DEPTH_LOGI("THREAD->StereoRectify: %6.3fms", 1000*(stop-start)/freq);
        start = stop;
        if (!ComputeDepth()) return 0;
        stop = TICK();
        VIDEO_DEPTH_LOGI("THREAD->ComputeDepth: %6.3fms", 1000*(stop-start)/freq);
        start = stop;
        if (!Postprocess()) return 0;
        stop = TICK();
        VIDEO_DEPTH_LOGI("THREAD->Postprocess: %6.3fms", 1000*(stop-start)/freq);
        return 1;
    }

    // down-sample to reduce estimate time of movement
    start = TICK();
    if (thread_state!=THREAD_STATE_PLAY) return 0;
    cv::resize(inputGray2, curr, opt_size);
    if (thread_state!=THREAD_STATE_PLAY) return 0;
    stop = TICK();
    VIDEO_DEPTH_LOGI("CalcDenseOptFlow->Preprocess: %6.3fms", 1000*(stop-start)/freq);

    if (m_param.dump_enabled) DumpImage(curr, m_param.dump_path, "curr", uid);
    if (m_param.dump_enabled) DumpImage(prev, m_param.dump_path, "prev", uid);

    start = TICK();
    float res_level = 0.5;
    int num_worst = 3;
    cv::Mat prev_aligned, curr_aligned;
    RotateCode rotate_code;
    float v_x, v_y;
    if (thread_state!=THREAD_STATE_PLAY) return 0;
    int status = RowAlign_xiang(
            v_x, v_y,
            curr_aligned, prev_aligned,
            curr, prev,
            res_level, num_worst,
            &thread_state, rotate_code, real_max_disp);
    // update frame stack
    curr.copyTo(prev);
    stop = TICK();
    VIDEO_DEPTH_LOGI("CalcDenseOptFlow->RowAlign_xiang: %6.3fms", 1000*(stop-start)/freq);
	
	status = false;
    if(!status) {
		VIDEO_DEPTH_LOGI("====== CAMERA STEADY ======");
        // monocular failed, then run stereo depth estimate algorithm or 
		// simply interpolate a new disparity map with fresh main
		freeze_times = (freeze_times + 1) % m_param.skip_cycle;
		if (freeze_times) { // directly interpolate new disparity
			start = TICK();
			cv::Mat edges;
			GetFullEdges(edges, depth_base, 10, 5);
			// make the edge zone
			cv::Mat process_region;
			cv::dilate(edges, process_region, cv::Mat(), cv::Point(-1, -1), 3);
			if (m_param.dump_enabled) DumpImage(depth_base, m_param.dump_path, "base", uid);
			if (m_param.dump_enabled) DumpImage(edges, m_param.dump_path, "edges", uid);
			if (m_param.dump_enabled) DumpImage(process_region, m_param.dump_path, "zone", uid);
			stop = TICK();
			VIDEO_DEPTH_LOGI("THREAD->GetFullEdges: %6.3fms", 1000 * (stop - start) / freq);
			
			// fill in the edge zone
			start = stop;
			depth_out = depth_base.mul(1 - process_region / 255);
			cv::Mat rgb_main_small;
			YUV2RGB_simple(rgb_main, inputGray2, inputU2, inputV2);
			cv::resize(rgb_main, rgb_main_small, depth_base.size());
			FillBlackHoles(depth_out, rgb_main_small);
			if (m_param.dump_enabled) DumpImage(depth_out, m_param.dump_path, "depth_inter", uid);
			stop = TICK();
			VIDEO_DEPTH_LOGI("THREAD->Interpolation: %6.3fms", 1000 * (stop - start) / freq);

			// postprocess the depth map
			start = TICK();
			cv::Mat main_wls, disp_1, disp_2;
			if (thread_state != THREAD_STATE_PLAY) return 0;
			cv::resize(rgb_main_small, main_wls, wls_size);
			if (thread_state != THREAD_STATE_PLAY) return 0;
			cv::resize(depth_out, disp_1, wls_size);
			stop = TICK();
			VIDEO_DEPTH_LOGI("CalcDenseOptFlow->Resize: %6.3fms", 1000 * (stop - start) / freq);

			start = TICK();
			wls_filter->setSigmaColor(3);
			wls_filter->setLambda(800);
			cv::medianBlur(main_wls, main_wls, 5);
			if (m_param.dump_enabled) DumpImage(main_wls, m_param.dump_path, "main_wls", uid);
			if (thread_state != THREAD_STATE_PLAY) return 0;
			wls_filter->filter(disp_1, main_wls, disp_2);
			
			stop = TICK();
			VIDEO_DEPTH_LOGI("CalcDenseOptFlow->wls_filter1: %6.3fms", 1000 * (stop - start) / freq);
			if (m_param.dump_enabled) DumpImage(disp_2, m_param.dump_path, "disp_inter_1", uid);
		

			/*start = TICK();
			wls_filter->setSigmaColor(0.4);
			wls_filter->setLambda(8000);
			if (thread_state != THREAD_STATE_PLAY) return 0;
			wls_filter->filter(disp_2, disp_2, disp_1);
			stop = TICK();
			VIDEO_DEPTH_LOGI("CalcDenseOptFlow->wls_filter2: %6.3fms", 1000 * (stop - start) / freq);
			if (m_param.dump_enabled) DumpImage(disp_1, m_param.dump_path, "disp_inter_2", uid);*/

			HistBasedFilter(disp_2, 0.04, false);

			start = TICK();
			if (thread_state != THREAD_STATE_PLAY) return 0;
			cv::resize(disp_2, depth_out, m_param.depth_size);
			stop = TICK();
			VIDEO_DEPTH_LOGI("CalcDenseOptFlow->resize: %6.3fms", 1000 * (stop - start) / freq);

			while (map_locked);
			map_locked = true;
			depth_out.copyTo(depth_last);
			map_locked = false;
			return 1;
		}
		else { // run stereo depth estimate
			start = TICK();
			if (!Preprocess()) return 0;
			stop = TICK();
			VIDEO_DEPTH_LOGI("THREAD->Preprocess: %6.3fms", 1000 * (stop - start) / freq);
			start = stop;
			if (!StereoRectify()) return 0;
			stop = TICK();
			VIDEO_DEPTH_LOGI("THREAD->StereoRectify: %6.3fms", 1000 * (stop - start) / freq);
			start = stop;
			if (!ComputeDepth()) return 0;
			stop = TICK();
			VIDEO_DEPTH_LOGI("THREAD->ComputeDepth: %6.3fms", 1000 * (stop - start) / freq);
			start = stop;
			if (!Postprocess()) return 0;
			stop = TICK();
			VIDEO_DEPTH_LOGI("THREAD->Postprocess: %6.3fms", 1000 * (stop - start) / freq);

			// to keep the same as the edge-refined depth, apply the procedure here too.
			cv::Mat edges;
			GetFullEdges(edges, depth_base, 10, 5);
			cv::Mat process_region;
			cv::dilate(edges, process_region, cv::Mat(), cv::Point(-1, -1), 3);
			depth_out = depth_base.mul(1 - process_region / 255);
			cv::Mat rgb_main_small;
			YUV2RGB_simple(rgb_main, inputGray2, inputU2, inputV2);
			cv::resize(rgb_main, rgb_main_small, depth_base.size());
			FillBlackHoles(depth_out, rgb_main_small);
			cv::Mat main_wls, disp_1, disp_2;
			if (thread_state != THREAD_STATE_PLAY) return 0;
			cv::resize(rgb_main_small, main_wls, wls_size);
			if (thread_state != THREAD_STATE_PLAY) return 0;
			cv::resize(depth_out, disp_1, wls_size);
			wls_filter->setSigmaColor(3);
			wls_filter->setLambda(800);
			cv::medianBlur(main_wls, main_wls, 5);
			if (m_param.dump_enabled) DumpImage(main_wls, m_param.dump_path, "main_wls", uid);
			if (thread_state != THREAD_STATE_PLAY) return 0;
			wls_filter->filter(disp_1, main_wls, disp_2);
			HistBasedFilter(disp_2, 0.04, false);
			cv::resize(disp_2, depth_out, m_param.depth_size);
			while (map_locked);
			map_locked = true;
			depth_out.copyTo(depth_last);
			map_locked = false;
			
			return 1;
		}
    }
	
	// reset the camera-steady counter
	VIDEO_DEPTH_LOGI("====== CAMERA MOVED ======");
	freeze_times = 0;

    if (m_param.dump_enabled) DumpImage(curr_aligned, m_param.dump_path, "main_aligned", uid);
    if (m_param.dump_enabled) DumpImage(prev_aligned, m_param.dump_path, "aux_aligned", uid);

    start = TICK();
    if (thread_state!=THREAD_STATE_PLAY) return 0;
    cv::flip(curr_aligned, curr_aligned, -1);
    if (thread_state!=THREAD_STATE_PLAY) return 0;
    cv::flip(prev_aligned, prev_aligned, -1);

    //int max_disp = 16;
    bm->setNumDisparities(m_param.max_disp);
    if (thread_state!=THREAD_STATE_PLAY) return 0;
    bm->compute(curr_aligned, prev_aligned, depth_s16);
    cv::threshold(depth_s16, depth_s16, real_max_disp*16, 0, cv::THRESH_TOZERO_INV);
    bm->setNumDisparities(m_param.max_disp);
    depth_s16.convertTo(depth_u8, CV_8UC1, 255.0/(m_param.max_disp*16));
    stop = TICK();
    VIDEO_DEPTH_LOGI("CalcDenseOptFlow->ComputeDepth: %6.3fms", 1000*(stop-start)/freq);
    if (m_param.dump_enabled) DumpImage(depth_u8, m_param.dump_path, "disp_0", uid);

    // remove white spots
    start = TICK();
    if (thread_state!=THREAD_STATE_PLAY) return 0;
    HistBasedPeakFilter(depth_u8, 0.007, 16);
    stop = TICK();
    VIDEO_DEPTH_LOGI("CalcDenseOptFlow->HistBasedPeakFilter: %6.3fms", 1000*(stop-start)/freq);
    if ( m_param.dump_enabled ) DumpImage(depth_u8, m_param.dump_path, "disp_1", uid);

    // fill the rest black holes
    start = TICK();
    if (thread_state!=THREAD_STATE_PLAY) return 0;
    FillBlackHoles_Gray(depth_u8, curr_aligned);
    if (thread_state!=THREAD_STATE_PLAY) return 0;
    FillBlackHoles_Gray(depth_u8, curr_aligned);
    stop = TICK();
    VIDEO_DEPTH_LOGI("CalcDenseOptFlow->FillBlackHoles_Gray: %6.3fms", 1000*(stop-start)/freq);
    if ( m_param.dump_enabled ) DumpImage(depth_u8, m_param.dump_path, "disp_2", uid);

	HistBasedFilter(depth_u8, 0.04, false);

    // restore disparity map orientation
    start = TICK();
    if (thread_state!=THREAD_STATE_PLAY) return 0;
    cv::flip(depth_u8, depth_u8, -1);

    if ( rotate_code == ROTATE_CODE_90) {
        if (thread_state!=THREAD_STATE_PLAY) return 0;
        cv::rotate(depth_u8, depth_u8, cv::ROTATE_90_COUNTERCLOCKWISE);
    }
    else if (rotate_code == ROTATE_CODE_180) {
        if (thread_state!=THREAD_STATE_PLAY) return 0;
        cv::flip(depth_u8, depth_u8, -1);
    }
    else if (rotate_code == ROTATE_CODE_270) {
        if (thread_state!=THREAD_STATE_PLAY) return 0;
        cv::rotate(depth_u8, depth_u8, cv::ROTATE_90_CLOCKWISE);
    }
    stop = TICK();
    VIDEO_DEPTH_LOGI("CalcDenseOptFlow->Rotate: %6.3fms", 1000*(stop-start)/freq);

    // postprocess the depth map
    start = TICK();
    cv::Mat curr_small, disp_1, disp_2;
    if (thread_state!=THREAD_STATE_PLAY) return 0;
    cv::resize(curr, curr_small, wls_size);
    if (thread_state!=THREAD_STATE_PLAY) return 0;
    cv::resize(depth_u8, disp_1, wls_size);
    stop = TICK();
    VIDEO_DEPTH_LOGI("CalcDenseOptFlow->Resize: %6.3fms", 1000*(stop-start)/freq);

    start = TICK();
    wls_filter->setSigmaColor(3);
    wls_filter->setLambda(800);
    if (thread_state!=THREAD_STATE_PLAY) return 0;
    wls_filter->filter(disp_1, curr_small, disp_2);
    stop = TICK();
    VIDEO_DEPTH_LOGI("CalcDenseOptFlow->wls_filter1: %6.3fms", 1000*(stop-start)/freq);
    if (m_param.dump_enabled) DumpImage(disp_2, m_param.dump_path, "disp_3", uid);

    start = TICK();
    wls_filter->setSigmaColor(0.4);
    wls_filter->setLambda(8000);
    if (thread_state!=THREAD_STATE_PLAY) return 0;
    wls_filter->filter(disp_2, disp_2, disp_1);
    stop = TICK();
    VIDEO_DEPTH_LOGI("CalcDenseOptFlow->wls_filter2: %6.3fms", 1000*(stop-start)/freq);

    if (m_param.dump_enabled) DumpImage(disp_1, m_param.dump_path, "disp_4", uid);

    start = TICK();
    if (thread_state!=THREAD_STATE_PLAY) return 0;
    cv::resize(disp_1, depth_base, m_param.depth_size);
    stop = TICK();
    VIDEO_DEPTH_LOGI("CalcDenseOptFlow->resize: %6.3fms", 1000*(stop-start)/freq);

    while (map_locked);
    map_locked = true;
    depth_base.copyTo(depth_last);
    map_locked = false;
    return 1;
}

bool tcl_depth_video::TCLStereoDepthAlg_prv::Available() {
	return this->init_success;
}